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ABSTRACT 

Constant  speed  paths  have  been  used  as  interpolating  functions  for  highway 
and  railway  design,  motion  planning  for  robots,  and  path  planning  for  military 
aircraft.  A  simple  algorithm  is  developed  in  this  note  to  compute  a  two- 
dimensional  path  that  interpolates  between  ordered  data  points,  consisting  of 
Cornu-spiral,  straight-line  and  circular  sub-paths.  The  interpolation  algorithm 
determines  the  sub-path  parameters  such  that  the  interpolating  path  has  con¬ 
stant  speed,  continuous  heading  and  bounded  curvature,  while  endeavouring 
to  sequentially  minimise  the  sub-path  arc  lengths. 

The  most  signihcant  factor  to  influence  the  performance  of  the  interpolation 
algorithm  is  the  number  of  data  points  that  require  the  interpolating  path  to 
execute  a  hard  turn.  If  no  hard  turns  are  required,  then  the  interpolation 
algorithm  quickly  returns  an  interpolating  path  with  minimal  arc  length  (in  a 
heuristic  sense).  However,  hard  turns  can  substantially  increase  both  the  CPU 
time  of  the  algorithm  and  arc  length  of  the  path. 
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Constant  Speed  Interpolating  Paths 

Executive  Summary 


Constant  speed  paths  have  been  used  as  interpolating  functions  for  highway  and  railway 
design,  motion  planning  for  robots,  and  path  planning  for  unmanned  aerial  vehicles.  This 
note  emerged  from  a  study  of  path  planning  for  military  aircraft  conducting  missions  in 
hostile  environments,  where  the  interpolating  path  represents  the  trajectory  of  the  aircraft 
as  it  flies  through  waypoints. 

A  simple  algorithm  is  developed  to  compute  a  two-dimensional  path  that  interpolates 
between  ordered  data  points.  The  heading  of  each  sub-path  is  represented  by  a  quadratic 
polynomial,  resulting  in  Cornu-spiral,  straight-line  and  circular  sub-paths.  The  interpo¬ 
lation  algorithm  determines  the  sub-path  parameters  such  that  the  interpolating  path 
has  constant  speed,  continuous  heading  and  bounded  curvature,  while  endeavouring  to 
sequentially  minimise  the  sub-path  arc  lengths. 

Stoer^  used  Lagrange  multipliers  and  Newton’s  method  to  calculate  an  interpolating 
path  with  constant  speed,  continuous  heading  and  curvature,  and  minimal  total  arc  length. 
This  incurs  a  significant  computational  cost.  A  simpler  approach  is  taken  in  this  note, 
where  the  curvature  of  the  interpolating  path  is  permitted  to  have  a  finite  jump  disconti¬ 
nuity  at  the  data  points,  and  the  arc  length  of  each  sub-path  is  attempted  to  be  minimised 
in  sequence. 

The  interpolation  algorithm  developed  in  this  note  begins  by  returning  the  straight-line 
sub-path  between  the  first  two  data  points.  Then  for  each  of  the  remaining  data  points: 

1.  Update  the  initial  heading. 

2.  If  the  initial  heading  and  the  heading  of  the  straight-line  sub-path  are  equal,  then 
return  the  straight-line  sub-path. 

3.  If  the  straight-line  sub-path  was  not  returned,  then: 

(i)  Calculate  the  circular  sub-path. 

(ii)  Attempt  to  minimise  the  arc  length  of  the  Cornu-spiral  sub-path. 

(iii)  If  a  Cornu-spiral  sub-path  was  not  able  to  be  calculated  or  the  arc  length  of 
the  circular  sub-path  is  less  than  the  arc  length  of  the  Cornu-spiral  sub-path, 
then  return  the  circular  sub-path,  otherwise  return  the  Cornu-spiral  sub-path. 

4.  Return  to  the  first  step  and  repeat  until  the  interpolating  path  is  complete. 


The  most  significant  factor  to  influence  the  performance  of  this  interpolation  algorithm 
is  the  number  of  data  points  that  require  the  interpolating  path  to  execute  a  hard  turn. 
If  no  hard  turns  are  required,  then  the  interpolation  algorithm  quickly  returns  an  inter¬ 
polating  path  with  minimal  arc  length  (in  a  heuristic  sense).  However,  hard  turns  can 
substantially  increase  both  the  CPU  time  of  the  algorithm  and  arc  length  of  the  path. 


Although  this  note  emerged  from  a  study  of  path  planning  for  military  aircraft,  the 
interpolation  algorithm  is  generic  and  potentially  has  a  broad  range  of  applications.  The 
utility  of  the  interpolation  algorithm  will  be  determined  by  the  context  in  which  the  data 
is  generated  and  the  intended  use  of  the  interpolating  path. 


^Stoer,  J.  (1982)  Curve  fitting  with  clothoidal  splines,  Journal  of  Research  of  the  National  Bureau  of 
Standards  87(4),  317-346. 
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Notation 


X 

|x| 

xy 

fiz) 

p 

oo 


N 


Xj 


s 


Pi 

ii 

£ 

Oi 

% 

Qi 

bi 

Ci{s) 

S^{s) 

Fc{s) 

Fs{s) 

Ct{s)^  C-{s) 
St{s)^  S-{s) 

Axj 

X? 

^Pi 

Ki 

{cLi,  £i) 

M{0,aa) 

ea 

0 


a  generic  2-d  vector,  x  =  {xi,X2) 

magnitude  of  x,  |x|  = 

scalar  (dot)  product,  x  •  y  =  xiyi  +  X2y2 

^ ,  the  derivative  of  a  function  /  with  respect  to  z 

interpolating  path 

infinity 

real  numbers 

number  of  data  points 

(xj.  Hi),  the  ith  data  point 

arc  length  as  a  parameter 

heading  of  p 

sub-path  of  p  between  Xj  and  Xj+i 
arc  length  of  p* 
arc  length  of  p 

heading  of  pi,  9i{s)  =  6q  +  aiS  +  biS^ 
initial  heading  of  p* 
initial  curvature  of  p* 

controls  the  rate  at  which  the  curvature  of  p*  changes 
first  component  of  the  Cornu-spiral  representation  of  pi 
second  component  of  the  Cornu-spiral  representation  of  pi 
cosine  Fresnel  Integral 
sine  Fresnel  Integral 

value  of  Ci{s)  when  6*  >  0  and  bi  <  0,  respectively 
value  of  Si{s)  when  >  0  and  hi  <  0,  respectively 

Xi+l  -  Xj 

centre  of  the  ith  circular  sub-path 
angle  between  Xj+i  —  x?  and  Xj  —  xf 
curvature  of  pj 

initial  guess  for  a  solution  {ai,£i)  to  {Ci{£i),  Si{£i))  =  Xj+i  —  Xj 
normal  probability  density  function  with  zero  mean  and  standard  deviation  aa 
|ai|  >  Ca  where  Ca  <C  1 
empty  set 
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1  Introduction 

In  a  typical  interpolation  problem,  a  finite  set  of  discrete  data  points  is  given  and  a 
continuous  interpolating  function  that  passes  through  each  of  the  data  points  must  be 
determined  [Atkinson  1989] .  The  distinguishing  features  of  a  good  interpolating  function 
depend  on  the  context  in  which  the  data  was  generated  and  the  intended  use  of  the 
interpolating  function.  Constant  speed  paths  have  been  used  as  interpolating  functions  for 
highway  and  railway  design  [Meek  &  Walton  1989,  Meek  &  Walton  1992],  motion  planning 
for  robots  [Kelly  &  Nagy  2003],  and  path  planning  for  unmanned  aerial  vehicles  [Dai  & 
Cochran  Jr.  2010].  This  note  emerged  from  a  study  of  path  planning  for  military  aircraft 
conducting  missions  in  hostile  environments,  where  the  interpolating  path  represents  the 
trajectory  of  the  aircraft  as  it  flies  through  waypoints. 

The  aim  of  this  note  is  to  develop  a  simple  algorithm  to  compute  a  path  p :  [0,  oo)  — > 
that  interpolates  between  N  ordered  data  points  C  This  interpolating  path 

must  satisfy  the  following  properties: 

1.  Interpolation;  the  path  passes  through  all  the  data  points. 

2.  Constant  speed;  the  path  has  a  given  constant  speed  at  each  point  on  the  path. 

3.  Continuous  heading;  the  heading  of  the  path  is  continuous  at  each  point  on  the 
path.^ 

4.  Bounded  curvature;  the  curvature  of  the  path  is  bounded  at  each  point  on  the 
path. 

It  is  also  desirable  for  the  path  have  minimal  length,  subject  to  the  constraints  imposed 
by  Properties  1  to  4. 

Let  p  be  parametrised  by  arc  length  s  G  [0,oo).  Then  Property  2  implies  that  p  has 
the  form  ^ 

p(s)  =  xi  +  /  {cos  9 (z),  sin  9 (z))  dz,  (1) 

Jo 

where  9{s)  is  the  heading  of  the  path  when  its  arc  length  is  s  and  jpj  =  1,  that  is,  p  has 
unit  speed. ^  It  remains  to  choose  9{s)  such  that  Properties  1  to  4  are  satisfied.  If  the 
heading  is  represented  by  a  polynomial  in  s,  then  the  resulting  paths  are  known  as  Cornu 
spirals  (see  Figure  1),  which  have  been  employed  as  interpolating  paths  with  minimal 
length  [Stoer  1982]  and  satisfying  a  least-squares  criteria  [Davis  1999],  for  highway  and 
railway  design  [Meek  &  Walton  1989,  Meek  &  Walton  1992],  motion  planning  for  robots 
[Kelly  &  Nagy  2003],  and  path  planning  for  unmanned  aerial  vehicles  [Dai  &  Cochran 
Jr.  2010]. 

The  algorithm  developed  in  this  note  to  compute  p  is  based  on  the  work  of  Stoer 
[1982].  The  interpolating  path  computed  by  Stoer  has  continuous  curvature  at  each  point 
on  the  path  and  the  integral  of  the  curvature  squared  over  the  length  of  the  entire  path 

^For  a  path  with  continuous  heading,  infinitesimal  changes  in  position  on  the  path  result  in  infinitesimal 
changes  in  heading. 

^Any  constant  speed  path  can  be  re-parametrised  by  arc  length  to  yield  a  unit  speed  path. 
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Figure  1:  An  example  of  a  Cornu  spiral. 


is  minimised,  which  is  equivalent  to  minimal  total  arc  length.  This  incurs  a  significant 
computational  cost.  Here  the  emphasis  is  on  a  simple  algorithm  to  compute  p,  and  as  a 
result  these  two  properties  are  relaxed: 

•  The  curvature  of  p  is  permitted  to  have  a  finite  jump  discontinuity  at  the  data 


points. 


•  The  integral  of  the  curvature  squared  over  the  length  of  p  between  each  of  the  data 
points  is  attempted  to  be  minimised  in  sequence. 

These  properties  are  consistent  with  Properties  1  to  4.  Note  that  p  is  not  guaranteed  to 
have  minimal  total  arc  length. 

In  Section  2  a  representation  of  the  interpolating  path  in  terms  of  Cornu-spiral,  straight- 
line  and  circular  sub-paths  is  stated  and  discussed.  The  interpolation  algorithm  is  devel¬ 
oped  in  Section  3,  including  a  derivation  of  the  sub-path  arc  length  minimisation  heuristic 
that  is  central  to  the  algorithm.  In  Section  4  the  interpolation  algorithm  is  specified  us¬ 
ing  pseudo  code  and  its  performance  with  respect  to  CPU  time  and  total  arc  length  is 
illustrated. 


2  Representation  of  the  Interpolating  Path 


Let  Pi  :  [0,ii]  — >  be  the  sub-path  of  p  between  Xj  and  Xj+i  with  arc  length  C  for 

i  =  1, . . . ,  N  —  1.  Denote  the  total  arc  length  of  p  by  i,  then 


N-l 


1  =  1 


2 
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Stoer  [1982]  represents  the  heading  of  each  sub-path  9i  :  [O,^*]  ^  M  by  a  quadratic 
polynomial, 

9i{s)  =  Oq  aiS  -|-  (2) 

for  i  =  —  1,  resulting  in  sub-paths  that  are  Cornu  spirals  (see  Figure  1).  The  sub¬ 

path  parameters  0g,  a*  and  bi  determine  the  shape  of  the  path:  6q  is  the  initial  heading, 
tti  is  the  initial  curvature,  and  bi  controls  the  rate  at  which  the  curvature  changes  as  s 
increases.  Special  cases  include  straight-line  (a,  =  6*  =  0)  and  circular  {bi  =  0)  sub-paths. 
Unlike  straight-line  and  circular  paths.  Cornu-spiral  paths  can  perform  one  or  two  turns. 
Although  Properties  1  to  4  can  be  satisfied  using  only  straight-line  and  circular  sub-paths, 
this  will  often  generate  interpolating  paths  with  excessively  long  arc  lengths. 

A  significant  advantage  of  this  representation  of  the  heading  is  that  an  analytical  form 
of  Equation  (1)  is  available  in  terms  of  Fresnel  Integrals  [Stoer  1982]: 

Pi(s)  =  Xj -h  (C'i(s),  S'j(s))  ,  (3) 

for  i  =  l,...,A^  —  1,  where  the  functions  Ci{s)  and  Si{s)  are  given  by 


and  Fc{z)  and  Fs{z)  are  the  Fresnel  Integrals^  [Abramowitz  &  Stegun  1972] 

Fc{z)  =  J  dt, 


It  can  be  shown  that  Ci{s)  and  Si{s)  are  bounded  and  real  for  all  parameter  values, 
including  bi  ^  0.  However,  this  relies  on  the  symmetry  relations  of  the  Fresnel  Integrals 
to  eliminate  complex  values,  and  the  limiting  behaviour  of  Ci{s)  and  Si{s)  as  bi  ^  0 
is  highly  oscillatory.  Therefore  numerical  computations  involving  Ci{s)  and  Si{s)  can 
be  problematic.  To  overcome  these  numerical  difficulties,  Ci{s)  and  Si{s)  are  given  the 
following  equivalent  definitions:^ 


a(s) 


C+(s),  bi>0 
C~  (s)  ,  bi  <0, 


(4) 


®An  example  of  code  to  compute  the  Fresnel  Integrals  can  be  found  at  MATLAB  Central: 
http : //www.mathworks . com/matlabcentral/f ileexchange/6580-f resnel-integrals  (accessed  Decem¬ 
ber  2010). 

^These  definitions  were  obtained  using  the  symmetry  relations  of  the  Fresnel  Integrals  [Abramowitz  & 
Stegun  1972]. 
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where 


at  \  /  „  /  Oj  +  2biS 


2 

4h 


cos(7f-0o) 


\/2Trbi 


at  ^4  \  /  ^  /  ai  +  2biS 


2 

4h 


y/2TTbi 


-Fs 


\/27rbi 


C-{s)  =  - 


TT 


2|6, 


eos(i-eh  FcUl^  -FcM^ 
46i  /  V  V  Ujt  |(>i  y  I  V^27rpi^ 


—  sin 


at  \  /  _  /  ay  +  2biS 


Ah 


'0  1  I  I 


-Fs 


di 


\/27r|6^|  /  w27r|6, 


and 


5y(s)  = 


'5+(s),  6,  >0 
S~(s),  bi<0, 


(5) 


where 


S,+(s)  = 


»s|g--95||Fsl!?li2^|-Fs 


y/27rh  J  \yj2TTbi 


at  „4  \  I  „  /  ai  +  2biS 


-smljJ-OJIlFo 


\/27rbi 


S-is)  = 


TT 


2\h 


46y  /  V  V  \/27r|6i|  i  I  v^27r|6y| 


a?  ^i  \  I  „  I  ai  +  2biS 


+  (Fc 


-Fc 


\/27r|6i|  y  V\/27r|6, 


Straight-line  (a*  =  6*  =  0)  and  circular  {h  =  0)  sub-paths  are  considered  separately 
due  to  the  singular  behaviour  of  Ci{s)  and  Si{s)  as  h  0,  and  the  availability  of  explicit 
analytic  formulae  for  these  paths.  Straight-line  sub-paths  have  the  form 

Pi(s)  =  Xj  +  ^(Xj+l  -  Xj), 

ti 

ii  =  |xi+i  -  Xi|  , 

for  s  G  [0,£i].  Circular  sub-paths  are  defined  by 

Pj(s)  =  Xj  +  —  (sin(0o  +  ais)  -  sin^g,  cosOq  -  cos[9q  -|-  ays))  ,  (8) 

di 


(6) 

(7) 
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for  s  €  [0,£i]  where 


Axj  •  (sin6»^,  -  cos  6*^) 

a*  =  -2 - ^ 

|Axj| 

Axj  =  Xj+i  -  Xi, 

Axi  ■  pi(0)  >  0 

Axj  •  pj(0)  ^  0, 

xf  =  Xj  -  —  (sin  0Q,  -  cos  0q)  , 

Cli 

Pi(0)  =  (cos  6*0,  sin6»o)  . 

Note  that  =  Xj+i  by  construction. 


arccos(a^(xj  -  xf)  •  (xj+i  -  xf))  , 

27r  -  arccos(ai  (xj  -  xf)  •  (xj+i  -  xf))  , 


(9) 

(10) 

(11) 

(12) 

(13) 

(14) 


3  Algorithm  Development 

The  purpose  of  the  interpolation  algorithm  is  to  determine  the  sub-path  parameters 
{0Q,  ai,bi,£i)  such  that  Properties  1  to  4  (Interpolation,  Constant  Speed,  Continuous  Head¬ 
ing,  and  Bounded  Curvature)  are  satisfied,  while  endeavouring  to  sequentially  minimise 
the  sub-path  arc  length  4- 


3.1  Strategy 

The  sub-path  parameters  (0g,  a*,  bi,£i)  must  be  determined  for  each  of  the  N  —  1  sub-paths 
Pi  to  completely  define  the  interpolating  path  for  a  given  set  of  ordered  points  {x*}^^. 
Stoer  [1982]  used  Lagrange  multipliers  and  Newton’s  method  to  find  {0Q,ai,bi,£i)  such 
that  the  curvature  of  the  interpolating  path  is  continuous  at  the  data  points,  and  the  total 
arc  length  of  the  path  is  minimal.  This  incurs  a  significant  computational  cost. 

A  simpler  approach  is  taken  in  this  note,  where  the  arc  length  of  each  sub-path  is 
attempted  to  be  minimised  in  sequence.  If  continuity  of  the  curvature  at  the  data  points  is 
also  required,  as  in  Stoer  [1982],  then  substantial  oscillations  in  the  path  can  be  introduced. 
This  can  be  seen  by  the  following  argument.  The  shortest  distance  between  two  points 
is  a  straight  line,  implying  that  ai  =  bi  =  0.  Continuity  of  the  curvature  at  X2  then 
yields  02  =  0.®  It  follows  that  the  heading  of  p2  can  only  change  in  one  direction  that 
is  determined  by  the  sign  of  b2,  which  may  require  p2  to  form  a  spiral  to  ensure  that 

®The  curvature  at  the  data  points  will  be  continuous  if  Ui  =  at-i  +  2&i_iC-i  for  i  =  2, . . . ,  N  —  I,  which 
follows  from  the  derivative  of  Equation  (2)  with  respect  to  arc  length. 


UNCLASSIFIED 


5 


DSTO-TN-0989 


UNCLASSIFIED 


P2(^2)  =  X3.  Therefore,  allowing  a  finite  jump  discontinuity  in  the  curvature  at  the  data 
points  will  ameliorate  the  formation  of  spirals.® 

This  strategy  will  typically  result  in  an  asymmetric  interpolating  path,  that  is,  if  the 
order  of  the  data  points  is  reversed  then  a  different  path  will  be  generated.  This  is  a 
consequence  of  sequentially  minimising  the  sub-path  arc  lengths. 


3.2  Satisfying  the  Interpolating  Path  Properties 


Interpolation 

The  Interpolation  property  implies  that 

Pi(0)  =  Xj, 

(15) 

Pj(^j)  =  Xj+i, 

(16) 

for  i  =  1, . . . ,  N  —  1.  The  functional  form  of  pi  was  chosen  to  satisfy  Equation  (15) 
and  hence  this  equation  does  not  yield  any  additional  information  regarding  the  sub-path 
parameters.  Observe  that  Equation  (16)  provides  two  independent  equations  for  the  sub¬ 
path  parameters.'^ 


Constant  Speed 

The  sub-paths  satisfy  the  Constant  Speed  property  by  construction  and  hence  this  prop¬ 
erty  also  does  not  yield  any  additional  information  regarding  the  sub-path  parameters. 


Continnous  Heading 

Since  the  shortest  distance  between  two  points  is  a  straight  line,  the  first  sub-path  is 
simply  a  straight-line  path  between  xi  and  X2,  and  hence 

6<q  =  aiclani— — ,  (17) 

Vx2  -xij 

where  Xj  =  {xi,yi).  Eor  i  =  2, . . . ,  N  —  1,  the  Continuous  Heading  property  implies  that 

^0  ~  ^0  ^  (18) 

which  follows  from  Equation  (2). 


^Reducing  the  number  of  constraints  in  any  optimisation  problem  will  lead  to  an  improved  optimal 
value  for  the  objective  function. 

^Explicit  solutions  to  Equation  (16)  are  provided  by  Equation  (7)  for  straight-line  paths  and  Equa¬ 
tions  (9)  to  (14)  for  circular  paths. 
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Bounded  Curvature 

The  (signed)  curvature  of  p*  is  denoted  by  k*:  [0,£i]  ^  M  and 

Ki(s)  =  6i(s)  =  Oj  +  2biS. 


Therefore 

max  |Kj(s)|  =  max {|ai|  ,  \ai  +  26*41}  , 

se[o,C] 

and  so  Ki  is  bounded  for  finite  values  of  ai,  bi  and  4- 


3.3  Arc  Length  Minimisation 

In  this  section,  a  method  for  calculating  the  sub-path  parameters  (0Q,a*,6j,4)  of  Cornu- 
spiral  sub-paths  is  discussed.  Explicit  formulae  for  the  sub-path  parameters  of  straight-line 
and  circular  paths  are  given  by  Equations  (6)  to  (14)  and  Equations  (17)  and  (18). 

Strategy 

Equation  (16)  provides  two  implicit  equations  to  define  (0Q,aj,6*,4)  and  0q  is  given  ex¬ 
plicitly  by  Equations  (17)  and  (18).  To  completely  determine  each  p*,  another  equation  or 
condition  is  required  to  specify  one  of  a*,  6*  or  4-  Requiring  the  curvature  to  be  continuous 
at  the  data  points  will  not  be  successful  in  the  context  of  sequential  minimisation  of  the 
sub-path  arc  lengths.  Eurthermore,  p*  is  singular  as  6*  ^  0  and  hence  varying  6*  to  solve 
Equation  (16)  may  be  problematic.®  As  a  result  it  is  advantageous  to  fix  6*  then  employ 
Equation  (16)  to  specify  a*  and  4- 

Eor  each  6*  there  may  be  multiple  a*  and  4  that  solve  Equation  (16);  the  solution 
with  minimal  4  is  sought.  Equation  (16)  is  solved  using  a  root-finding  algorithm  such 
as  Newton’s  method,  which  begins  with  an  initial  guess  for  the  solution.  There  are  three 
factors  that  can  influence  the  convergence  of  the  root-finding  algorithm  to  a  solution  of 
Equation  (16)  with  minimal  4^ 

•  proximity  of  the  initial  guess  for  a*  to  a  solution  that  corresponds  to  minimal  4; 

•  proximity  of  the  initial  guess  for  4  to  a  solution  that  corresponds  to  minimal  4;  and 

•  a  condition  for  6*  that  favours  solutions  with  minimal 


A  Condition  for  hi 

A  condition  for  bi  is  obtained  by  seeking  to  minimise 


f(i  4 

/  K‘f{s)ds  = -£i 

IQ  O 


,  «  3  V  3  2 

6*4  +  -^di  I  -|- 


“If  ai  —  0  then  the  interpolating  path  is  not  singular  as  bi  —>  0. 
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which  is  equivalent  to  minimising  li? 
this  suggests  that 


Since  ds  is  a  sum  of  two  positive  terms, 


(19) 


will  result  in  solutions  of  Equation  (16)  with  minimal 


Initial  Guesses  for  a*  and  ii 

Let  di  and  ii  be  initial  guesses  for  a  solution  {ai,£i)  to  Equation  (16).  A  lower  bound  for  ii 
is  the  arc  length  of  the  straight-line  path  between  Xj  and  Xj+i,  for  which  ai  =  bi  =  0.  This 
suggests  £i  =  |xj+i  —  Xj|  and  di  =  0.  However,  since  p*  is  singular  as  6*  ^  0,  Equation  (19) 
implies  that  di  ^  0.  Despite  this,  it  is  still  necessary  for  |ai|  to  be  (in  some  sense)  small 
to  guide  the  root-finding  algorithm  to  a  solution  of  Equation  (16)  with  minimal 

The  root-finding  algorithm  is  not  guaranteed  to  converge  to  a  solution  of  Equation  (16) 
for  every  choice  of  di  and  ii,  and  so  di  and/or  ii  must  be  modified  for  each  attempt  to 
solve  Equation  (16).  Setting  ii  =  |xj_|_i  —  Xj|  and  varying  a*  has  proven  to  yield  the  best 
results.  Since  a  small  value  of  a*  corresponds  to  a  small  value  of  ii,  di  is  selected  from  a 
normal  probability  distribution  with  zero  mean: 

di  ~  AA(0,cJa)  such  that  |ai|  >  Cq, 

where  AA(0,  da)  denotes  the  normal  distribution  probability  density  function  with  zero 
mean  and  standard  deviation  da  >  0  and  Ca  <C  1.  Both  da  and  Ca  are  given  fixed  parame¬ 
ters. 


4  The  Interpolation  Algorithm 

In  this  section,  the  interpolation  algorithm  is  specified  using  pseudo  code  and  its  perfor¬ 
mance  with  respect  to  CPU  time  and  total  arc  length  is  illustrated. 


4.1  Description 

Cornu-spiral  sub-paths 

The  sequential  sub-path  arc  length  minimisation  strategy  described  in  Section  3.3  is 
implemented  in  Algorithm  1  to  determine  {ai,hi,ii)  of  Cornu-spiral  sub-paths,  for  i  = 
2, . . . ,  N  —  1.^^  Note  that: 


•  da,  Ca,  lUpperBound  and  MaxAttempts  are  independent  of  the  sub-paths,  that  is, 
they  remain  constant  for  every  call  to  Algorithm  1, 

Ki{s)  ds  is  a  monotone  increasing  function  of  A. 

^°An  alternative  condition  is  bi  =  —aijli,  in  which  case  j/’  fvf  (s)  ds  = 

^^The  first  sub-path  is  given  by  Equations  (6)  and  (7)  and  9q  is  given  by  Equation  (18)  for  i  —  2,. . . ,  N—1. 
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•  lUpperBound  ^  li  for  all  i  G  {1, . . .  ,N  —  1}, 

•  MaxAttempts  specifies  the  maximum  number  of  attempts  to  solve  Equation  (16), 

•  the  definitions  of  Ci{s)  and  Si{s)  given  by  Equations  (4)  and  (5)  must  be  used  to 
avoid  complex  values  entering  the  computations,  and 

•  Newton’s  method  exhibited  far  superior  performance  when  compared  with  the  Secant 
method. 


Algorithm  1:  Determine  the  sub-path  parameters  of  Cornu  spirals. 

Input:  6q,  ii,  Xj,  Xj+i,  for  i  =  2, . . . ,  N  —  1,  CTa,  Ca,  lUpperBound  and  MaxAttempts 
Output:  (oj,  bi,ii)  for  i  =  2, . . . ,  A  —  1 

RootVector  ^  0; 

n  <—  1; 

while  RootVector  =  0  and  n  ^  MaxAttempts  do 
dj  ~  AA(0,cJa)  such  that  |di|  >  ea] 
bi  i - 0.75  ai/Ii] 

Solve  Si{£i))  =  Xj+i  —  x*  for  using  Newton’s  method  with  {ai,£i) 

as  an  initial  guess,  such  that  ii  ^  ii  ^  lUpperBound; 

if  A  solution  has  been  found  then 
I  Store  the  solution  in  RootVector; 
end 

n  <—  n  -|-  1; 

end 

if  RootVector  ^  0  then 

I  bi,  f  j) 

else 

I  0 

end 
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The  Interpolation  algorithm 

The  Interpolation  algorithm,  which  is  presented  in  Algorithm  2,  begins  by  returning  the 
straight-line  sub-path  between  the  first  two  data  points.  Then  for  each  of  the  remaining 
data  points: 

1.  Update  the  initial  heading. 

2.  If  the  initial  heading  and  the  heading  of  the  straight-line  sub-path  are  equal,  then 
return  the  straight-line  sub-path. 

3.  If  the  straight-line  sub-path  was  not  returned,  then: 

(i)  Calculate  the  circular  sub-path. 

(ii)  Call  Algorithm  1  repeatedly  to  minimise  the  arc  length  of  the  Cornu-spiral 
sub-path. 

(iii)  If  a  Cornu-spiral  sub-path  was  not  able  to  be  calculated  or  the  arc  length  of 
the  circular  sub-path  is  less  than  the  arc  length  of  the  Cornu-spiral  sub-path, 
then  return  the  circular  sub-path,  otherwise  return  the  Cornu-spiral  sub-path. 

4.  Return  to  the  first  step  and  repeat  until  the  interpolating  path  is  complete. 

Note  that: 

•  the  Interpolation  algorithm  will  not  return  an  interpolating  path  if  there  exists  an 
iG{l,...,Ai  —  1}  such  that  Xj+i  =  Xj,  and  may  not  return  an  interpolating  path  if 

(xj+i  -Xj)  •  (sin  00,  -cos0o)  =  0, 

which  is  equivalent  to  a  reversal  in  direction, 

•  Maxiterations  specifies  the  maximum  number  of  calls  to  Algorithm  1, 

•  TerminationFactor  >  1, 

•  if  Algorithm  1  returns  with 

ii  ^  TerminationFactor  x  |xj+i  —  Xj|  , 
then  the  Interpolation  algorithm  will  stop  attempting  to  minimise  ii,  and 

•  the  notation  SolutionVector[i]  is  the  ith  component  of  SolutionVector. 

The  performance  of  the  Interpolation  algorithm  is  discussed  in  Section  4.2. 
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Algorithm  2:  The  Interpolation  algorithm. 


Input:  Maxiterations  and  TerminationFactor 

Output:  (0Q,  Qi,  bi,  ii)  for  i  =  1, . . . ,  A  —  1 

^  arctan(^||^^;  ai  ^  0;  6i  ^  0;  ii  ^  |x2  -  xi|; 

(0Q,  ai, 


i^2; 

while  i  ^  A  —  1  do 
SolutionVector  ^ 
StraightLineFlag 

£i  ^  |xj+i  -  Xi|; 


^  +  (ii-i£i-i  +  bi-i£f 


0  ~r  ~r 

if  (Xj+i  -  Xi)  COS  9q  +  {yi+i  -  yi)  sin  9q  =  £i  then 
StraightLineFlag  <—  1; 
ai  <-  0; 
hi  ^  0; 

£i  ^  k 

end 


if  StraightLineFlag  =  0  then 

Calculate  the  circular  sub-path’s  parameters  with  aCircle  given  by 
Equation  (9)  and  ICircle  given  by  Equation  (11); 

£i  ^  oo; 

j  ^  1; 

while  j  ^  Maxiterations  and  £i  >  TerminationFactor  *  £i  do 

Call  Algorithm  1  with  inputs  0q,  fj,  Xj  and  Xj+i,  and  store  the  output  in 
SolutionVector; 

if  SolutionVector  7^  0  and  £i  >  SolutionVector[3]  then 
ai  ^  SolutionVector[l]; 
bi  ^  SolutionVector [2]; 

£i  <—  SolutionVector[3]; 
end 

j  ^  j  + 1; 

end 

if  SolutionVector  =  0  or  ICircle  <  £i  then 
ai  <—  aCircle; 

6*^0; 

£i  <—  ICircle; 
end 
end 

{9li,ai,bi,£i) 

i  ^  i  +  l] 

end 
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Table  1:  Algorithm  1  parameter  values  used  to  generate  the  figures  and  tables  in  See- 
tion  4-2. 


CTa 

Ca 

lUpperBound 

MaxAttempts 

25 

10-12 

1 

75 

4.2  Performance 

The  Interpolation  algorithm’s  performance  is  illustrated  in  this  section,  including  the 
impact  of  the  Maxiterations  and  TerminationFactor  parameters  on  CPU  time  and  arc 
length. 

The  Algorithm  1  parameter  values  that  were  used  to  generate  the  results  in  this  section 
are  shown  in  Table  1.  The  smallest  values  of  aa  and  MaxAttempts  were  chosen  such  that 
the  reliability  of  Algorithm  1  was  not  compromised. 

Adding  Points  and  Hard  Turns 

The  most  significant  factor  that  influences  the  CPU  time  of  the  Interpolation  algorithm  is 
the  number  of  data  points  that  require  the  interpolating  path  to  execute  a  hard  turn.  If 
the  number  of  data  points  increases  such  that  no  hard  turns  are  required,  then  the  CPU 
time  will  increase  linearly  with  the  number  of  points  because  the  computations  to  calculate 
the  sub-path  parameters  are  independent.  However,  hard  turns  can  substantially  increase 
the  CPU  time  of  the  algorithm. 

The  effect  of  hard  turns  on  CPU  time  was  ascertained  by  comparing  the  baseline  case 
(Case  1)  shown  in  Figure  2  with  a  variation  to  the  baseline  (Case  2),  which  is  shown  in 
Figure  3.  It  can  be  seen  in  Table  2  that  the  95%  confidence  intervals^^  for  the  average 
CPU  time  are  contained  in  [0.15,  0.65]  seconds  for  Case  1,  for  each  value  of  Maxiterations. 
Case  2  is  identical  to  Case  1  except  that  X4  has  been  altered  to  force  hard  turns  from  X3  to 
X4  and  from  X4  to  X5.  The  impact  of  these  hard  turns  on  CPU  time  for  Case  2  is  shown  in 
Table  2,  where  the  95%  confidence  intervals  for  the  average  CPU  time  are  now  contained 
in  [18, 134]  seconds,  for  each  value  of  Maxiterations. 


^^The  Interpolation  algorithm  was  rnn  using  Mathematica  8. 0.0.0  (64-bit  Kernel)  on  an  Apple  iMac 
running  Mac  OS  X  10.6.5  with  a  2.66  GHz  Intel  Core  2  Duo  CPU  and  4  GB  of  RAM. 

^^The  95%  confidence  intervals  were  generated  by  100  runs  of  the  Interpolation  algorithm  for  each  value 
of  Maxiterations;  a  greater  number  of  runs  did  not  qualitatively  change  the  confidence  intervals. 
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Xl 

Figure  2:  Case  1,  the  baseline  ease,  whieh  is  given  by  xi  =  (—0.0022,0.22),  X2  = 
(0.14,0.24),  X3  =  (0.27,0.44),  X4  =  (0.47,0.37)  anc?  xg  =  (0.55,0.48). 


Figure  3:  Case  2.  This  ease  was  ehosen  to  demonstrate  the  effeet  of  hard  turns  on 
the  CPU  time  of  the  Interpolation  algorithm;  the  points  are  identieal  to  Case  1  with  the 
exeeption  of -x.4  =  (0.47,0.037). 
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Table  2:  95%  confidence  intervals  for  the  average  CPU  time  and  total  arc  length  for 
different  values  of  Maxiterations  with  TerminationFactor  =  1.2.  Case  1  is  shown  in 
Figure  2  and  Case  2  is  shown  in  Figure  3. 


Case 

Maxiterations 

CPU  Time  (sec) 

i 

3 

[0.19,0.48] 

[0.78,0.79] 

Case  1 

6 

[0.15,0.47] 

[0.78,0.79] 

18 

[0.23,0.65] 

[0.78,0.79] 

3 

[18,24] 

[1.5, 1.6] 

Case  2 

6 

[44,51] 

[1.5, 1.6] 

18 

[118,134] 

[1.5, 1.6] 

The  impact  of  hard  turns  on  total  arc  length  was  investigated  by  considering  three  data 
points  of  the  form  shown  in  Figure  4;  the  parameter  G  [0,  tt]  was  incremented  by  O.OItt. 
The  Interpolation  algorithm  was  run  once  for  each  value  of  with  TerminationFactor  = 
1.2  and  Maxiterations  =  3.  Between  X2  and  X3  the  Interpolation  algorithm  returned 
Cornu-spiral  sub-paths  for  (j)  G  [O.Olvr,  O.TSvr],  where  the  total  arc  length  increased  from 
0.50002  to  0.59841.  Whereas  for  G  [0.797r,  0.997r]  the  Interpolation  algorithm  returned 
circular  sub-paths  between  X2  and  X3,  where  the  total  arc  length  increased  from  1.26  to 
25.0.  Therefore,  if  Cornu-spiral  sub-paths  cannot  be  computed  due  to  the  existence  of 
points  that  require  the  interpolating  path  to  execute  a  hard  turn,  then  the  total  arc  length 
may  increase  rapidly.  Adding  intermediate  points  to  decrease  the  angle  of  the  turn  may 
reduce  this  effect. 


Figure  4-  Definition  diagram  showing  the  points  used  to  demonstrate  the  effect  of  hard 
turns  on  total  arc  length  for  different  values  of  fi:  xi  =  (0,0),  X2  =  (0.25,0)  and 
X3  =  (0.25, 0)  -|-  0.25  (cos  sin  cf)  for  cj)  G  [0,  vr] . 
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Varying  Maxiterations  and  TerminationFactor 

Increasing  Maxiterations  will  linearly  increase  the  CPU  time  of  the  Interpolation  algo¬ 
rithm,  if  TerminationFactor  does  not  terminate  the  algorithm.  If  TerminationFactor 
is  active,  then  increasing  Maxiterations  may  have  little  effect  on  CPU  time. 

Table  2  shows  that  increasing  Maxiterations  had  a  negligible  effect  on  reducing  the 
total  arc  lengths  for  Cases  1  and  2.  This  is  not  always  the  case,  which  was  observed  when 
the  Secant  method  was  used  in  Algorithm  1.  Furthermore,  configurations  of  points  may  ex¬ 
ist  where  increasing  Maxiterations  reduces  the  total  arc  length.  Hence,  for  completeness, 
Maxiterations  was  retained  in  the  Interpolation  algorithm. 

The  linear  dependence  of  CPU  time  on  Maxiterations,  and  the  considerable  reduction 
in  CPU  time  that  may  be  achieved  by  increasing  TerminationFactor,  are  exhibited  in  Ta¬ 
ble  3  where  the  analysis  of  Case  2  was  repeated  with  TerminationFactor  =  2.  Compared 
with  Table  2,  where  TerminationFactor  =  1.2,  the  95%  confidence  intervals  for  the  aver¬ 
age  CPU  time  have  decreased  approximately  linearly  for  each  value  of  Maxiterations.  For 
example,  with  Maxiterations  =  3,  |  [18,24]  =  [6.0, 8.0].  This  is  sufficiently  close  to  the 
corresponding  interval  reported  in  Table  3  to  suggest  that  increasing  TerminationFactor 
caused  the  Interpolation  algorithm  to  terminate,  predominantly,  after  a  single  call  to 
Algorithm  1.^®  These  substantial  reductions  in  CPU  times  have  been  attained  without 
increasing  the  total  arc  lengths. 


Table  3:  95%  confidence  intervals  for  the  average  CPU  time  and  total  arc  length 
for  Case  2,  which  is  shown  in  Figure  3,  for  different  values  of  Maxiterations  with 
TerminationFactor  =  2. 


Maxiterations 

CPU  Time  (sec) 

e 

3 

[7.0,11] 

[1.5, 1.6] 

6 

[5.4,  8.6] 

[1.5, 1.6] 

18 

[8.2,12] 

[1.5, 1.6] 

5  Conclusion 


A  simple  algorithm  has  been  developed  to  compute  a  two-dimensional  path  that  inter¬ 
polates  between  ordered  data  points.  The  interpolating  path  consists  of  Cornu-spiral, 
straight-line  and  circular  sub-paths  (refer  to  Section  2).  The  Interpolation  algorithm  (Al¬ 
gorithm  2  in  Section  4.1)  determines  the  sub-path  parameters  such  that  the  interpolating 
path  has  constant  speed,  continuous  heading  and  bounded  curvature,  while  endeavouring 
to  sequentially  minimise  the  sub-path  arc  lengths  (refer  to  Section  3). 

^"^Setting  Maxiterations  =  1  will  prevent  the  Interpolation  algorithm  from  attempting  to  further  min¬ 
imise  the  sub-path  arc  lengths  returned  by  Algorithm  1. 

^®To  disable  TerminationFactor,  set  TerminationFactor  =  1. 


UNCLASSIFIED 


15 


DSTO-TN-0989 


UNCLASSIFIED 


The  performance  of  the  Interpolation  algorithm  was  illustrated  in  Section  4.2,  where  it 
was  shown  that  the  most  significant  factor  to  influence  the  CPU  time  of  the  algorithm  and 
arc  length  of  the  interpolating  path  is  the  number  of  points  that  require  the  interpolating 
path  to  execute  a  hard  turn.  This  is  because  Algorithm  1  (in  Section  4.1)  is  more  likely 
to  fail  to  return  Cornu-spiral  sub-paths  for  hard  turns,  and  this  consumes  CPU  time. 
Furthermore,  if  Algorithm  1  fails  to  return  Cornu-spiral  sub-paths,  then  the  Interpolation 
algorithm  will  return  circular  sub-paths,  which  typically  have  much  greater  arc  lengths 
compared  with  Cornu-spiral  sub-paths.  However  if  no  hard  turns  are  required,  then  the 
Interpolation  algorithm  quickly  returns  an  interpolating  path  with  minimal  arc  length  (in 
a  heuristic  sense). 

Although  this  note  emerged  from  a  study  of  path  planning  for  military  aircraft,  the 
Interpolation  algorithm  is  generic  and  potentially  has  a  broad  range  of  applications.  The 
utility  of  the  Interpolation  algorithm  will  be  determined  by  the  context  in  which  the  data 
is  generated  and  the  intended  use  of  the  interpolating  path. 
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